clear all
close all
clc


%% Test CSM_Real_Concat

disp('- CSM_Real_Concat test');

% specify database
bin_root = ['..' ...
  filesep 'binarydb' ...
  filesep];
name = 'lfpw';
type = 'train';
ann = 'our';
db = DB([name '-' type '-' ann],bin_root);

% number of image
n_data = 100;

% get dataset
id = 1:n_data;
reg_exp_query = db.RegExpQuery(id);
ds = db.Load(reg_exp_query);

% create CSM_Real_Concat shape model
ann = ds.GetAnn();
part = ds.comp;
sm = CSM_Real_Concat(ann,part);

% reconstruct original shapes
rann = zeros(size(ann));
rec_err = zeros(ds.n_data,1);
for i = 1:ds.n_data
  rann(:,:,i) = sm.ProjectAnn(ann(:,:,i));
  rec_err(i) = mean(sum((ann(:,:,i) - rann(:,:,i)).^2,2));
end

% show originals and reconstructions
ori_color = 'green';
rec_color = 'red';
figure(1); 
for i = 1:ds.n_data
  clf
  hold on
  plot(ann(:,1,i),-ann(:,2,i), ...
  '*-', ...
  'Color',ori_color, ...
  'MarkerFaceColor',ori_color, ...
  'MarkerEdgeColor',ori_color, ... 
  'MarkerSize',4);
  plot(rann(:,1,i),-rann(:,2,i), ...
  '*-', ...
  'Color',rec_color, ...
  'MarkerFaceColor',rec_color, ...
  'MarkerEdgeColor',rec_color, ... 
  'MarkerSize',4);
  drawnow;
end

% test
tolerance = 10^-10;
assert( all(rec_err < tolerance*ones(size(rec_err))) && ...
  sm.n_vert == size(sm.mu_ann,1) && ...
  sm.n_vert == ds.n_vert && ...
  sum([sm.n_part_pc{:}]) == size(sm.GetPartPCsMatrixForm,2) && ...
  sum([sm.n_part_pc{:}]) == sum(sm.n_r) && ...
  sm.n_mass_pc == size(sm.mass_pc,2) && ...
  sm.n_mass_pc == sm.n_f && ...
  sm.n_sim_basis == size(sm.sim_basis,2) && ...
  sm.n_sim_basis == sm.n_q && ...
  sum([sm.n_part_pc{:}]) + sm.n_mass_pc == sm.n_b && ...
  sm.n_b + sm.n_sim_basis == sm.n_p)
disp('  passed');


%% Test CSM_Real_NWarp

disp('- CSM_Real_NWarp test');

% create CSM_Real_NWarp shape model
ann = ds.GetAnn();
part = ds.comp;
sm = CSM_Real_NWarp(ann,part);

% reconstruct original shapes
rann = zeros(size(ann));
rec_err = zeros(ds.n_data,1);
for i = 1:ds.n_data
  rann(:,:,i) = sm.ProjectAnn(ann(:,:,i));
  rec_err(i) = mean(sum((ann(:,:,i) - rann(:,:,i)).^2,2));
end

% show originals and reconstructions
ori_color = 'green';
rec_color = 'red';
figure(1); 
for i = 1:ds.n_data
  clf
  hold on
  plot(ann(:,1,i),-ann(:,2,i), ...
  '*-', ...
  'Color',ori_color, ...
  'MarkerFaceColor',ori_color, ...
  'MarkerEdgeColor',ori_color, ... 
  'MarkerSize',4);
  plot(rann(:,1,i),-rann(:,2,i), ...
  '*-', ...
  'Color',rec_color, ...
  'MarkerFaceColor',rec_color, ...
  'MarkerEdgeColor',rec_color, ... 
  'MarkerSize',4);
  drawnow;
end

% test
tolerance1 = 10^-10;
tolerance2 = 10^-10;
assert(all(rec_err < tolerance2*ones(size(rec_err))) && ...
  sm.n_vert == size(sm.mu_ann,1) && ...
  sm.n_vert == ds.n_vert && ...
  sum([sm.n_part_pc{:}]) == size(sm.GetPartPCsMatrixForm,2) && ...
  sum([sm.n_part_pc{:}]) == sum(sm.n_r) && ...
  sm.n_mass_pc == size(sm.mass_pc,2) && ...
  sm.n_mass_pc == sm.n_f && ...
  sm.n_sim_basis == size(sm.sim_basis,2) && ...
  sm.n_sim_basis == sm.n_q && ...
  sum([sm.n_part_pc{:}]) + sm.n_mass_pc  == sm.n_b && ...
  sm.n_b + sm.n_sim_basis == sm.n_p)
disp('  passed');

